
#include <string.h> 
#include "fifo.h"

void fifob_enqueue(fifob_t* f, uint8_t byte)
{
    uint32_t idx = f->wp & f->mask;
    f->pdata[idx] = byte;
    f->wp ++;
}
uint8_t fifob_dequeue(fifob_t* f)
{
    uint32_t idx = f->rp & f->mask;
    f->rp ++;
    return f->pdata[idx];
}
void fifob_enqueue_data(fifob_t* f, uint8_t* pdata, uint16_t bytes_to_write)
{
    uint32_t wp = f->wp & f->mask;
    uint32_t remain = f->mask + 1 - wp;
    if( remain > bytes_to_write )
    {
        memcpy( f->pdata + wp, pdata, bytes_to_write );        
    }
    else 
    {
        memcpy( f->pdata + wp, pdata, remain );        
        memcpy( f->pdata, pdata + remain, bytes_to_write - remain );
    }
    f->wp += bytes_to_write;
}
void fifob_dequeue_data(fifob_t* f, fifob_dequeue_t* d)
{
    uint32_t rp = f->rp & f->mask;
    uint32_t cnt = f->wp - f->rp;
    
    if( (rp + cnt) > (f->mask + 1) )
    {
        cnt = f->mask + 1 - rp;        
    }

    d->pdata = f->pdata + rp;
    d->count = cnt; 
    f->rp += cnt; 
}

uint32_t fifob_get_count(fifob_t* f)
{
    return f->wp - f->rp;
}

bool fifob_is_empty(fifob_t* f)
{
    return f->wp == f->rp; 
}

void fifob_reset(fifob_t* f)
{
    f->rp = f->wp;
}
